#include "encoder.h"          // 编码器应用头文件
#include "ti_msp_dl_config.h"     // TI MSP驱动库配置头文件
#include "headfile.h"  



int cnt;


volatile bool EncoderCnt_Flag_A = 1;//四倍频QEA触发切换标志位
volatile bool EncoderCnt_Flag_B = 1;//四倍频QEB触发切换标志位

uint32_t e1,e2;



// 定义两个编码器实例（A和B），volatile确保中断安全访问
volatile Encoder Encoder_A;
volatile Encoder Encoder_B;
unsigned char motorA_dir, motorB_dir; // 控制电机方向，1正，0反



// 定时器1中断服务函数（用于周期性处理编码器数据）定期计算编码器的计数，每固定一段时间触发中断
void TIMER_1_INST_IRQHandler(void)
{
    // 检查是否为零匹配中断，判断是否为定时器的溢出中断（设定的时间到了）触发
    if (DL_TimerG_getPendingInterrupt(TIMER_1_INST) == DL_TIMER_IIDX_ZERO) 
	{
        /* 更新最终编码器计数值 */
        //Encoder_A.Obtained_Get_Encoder_Count = Encoder_A.Should_Get_Encoder_Count;
        motorA.now = Encoder_A.Should_Get_Encoder_Count;

        // 注意：编码器B的最终值取反（可能是电机安装方向导致）
        motorB.now = Encoder_B.Should_Get_Encoder_Count;
        /* 重置实时计数器（实现增量计数） *///原始计数器清空，记录下一个脉冲数
        Encoder_A.Should_Get_Encoder_Count = 0;
        Encoder_B.Should_Get_Encoder_Count = 0;
        if(yaw)
        {
            if(start_flag && !position_status)PID_Overall_Contral(TRACK_SPEED);
            if(start_flag && position_status)PID_Overall_Contral(ANGLE_SPEED);
            //if(start_flag && position_status)
            // DL_GPIO_setPins(LED_PORT, LED_board_PIN);
        }
        // if(start_flag && !position_status)PID_Overall_Contral(TRACK_SPEED);
        if(position_status)DL_GPIO_setPins(LED_PORT, LED_board_PIN);
        if(!position_status)DL_GPIO_clearPins(LED_PORT, LED_board_PIN);

        cnt++;




    }
}






// 电机初始化函数（初始化电机控制的中断系统）开启编码器的中断响应功能
void Motor_Init(void)
{
    // 使能编码器中断
    NVIC_ClearPendingIRQ(ENCODER_INT_IRQN);  // 清除编码器中断挂起状态

    //NVIC_SetPriority(ENCODER_INT_IRQN, 5);   // 设置中断优先级（根据需求调整）
    NVIC_EnableIRQ(ENCODER_INT_IRQN);       // 使能PORTA中断

}




// 编码器GPIO中断服务函数（GROUP1）（电机转动时实时检测编码器的信号变化并记录转动的圈数）获得电机转的圈数和方向
void GROUP1_IRQHandler(void)
{
    uint32_t gpioA_interrupt;
    //uint32_t gpioB_interrupt = 0;
    
     // 获取gpioA_interrupt上编码器相关引脚的中断状态（E1A/E1B/E2A/E2B）
    gpioA_interrupt = DL_GPIO_getEnabledInterruptStatus(
        ENCODER_PORT,  // 编码器端口统一为gpioA_interrupt（来自ti_msp_dl_config.h）
        ENCODER_E1A_PIN | ENCODER_E1B_PIN | ENCODER_E2A_PIN | ENCODER_E2B_PIN
    );

        // 处理编码器B的E2A引脚中断（正交解码）
    if (gpioA_interrupt & ENCODER_E2A_PIN) 
    {
        e2++;
        // 读取E2B引脚电平判断方向（E2A与E2B为正交信号）
        if (!DL_GPIO_readPins(ENCODER_PORT, ENCODER_E2B_PIN)) 
        {
            Encoder_B.Should_Get_Encoder_Count--;  // 反转计数
        } 
        else 
        {
            Encoder_B.Should_Get_Encoder_Count++;  // 正转计数
        }
    }
    
    // 处理编码器B的E2B引脚中断（正交解码）
    if (gpioA_interrupt & ENCODER_E2B_PIN) 
    {
        e2++;
        // 读取E2A引脚电平判断方向
        if (!DL_GPIO_readPins(ENCODER_PORT, ENCODER_E2A_PIN)) 
        {
            Encoder_B.Should_Get_Encoder_Count++;  // 正转计数
        } 
        else 
        {
            Encoder_B.Should_Get_Encoder_Count--;  // 反转计数
        }
    }


    // 处理编码器A的E1A引脚中断（正交解码）
    if (gpioA_interrupt & ENCODER_E1A_PIN) 
    {
        e1++;
        // 读取E1B引脚电平判断方向（E1A与E1B为正交信号）
        if (!DL_GPIO_readPins(ENCODER_PORT, ENCODER_E1B_PIN))
        {
            Encoder_A.Should_Get_Encoder_Count--;  // 反转计数
        } 
        else 
        {
            Encoder_A.Should_Get_Encoder_Count++;  // 正转计数
        }
    }
    
    // 处理编码器A的E1B引脚中断（正交解码）
    if (gpioA_interrupt & ENCODER_E1B_PIN) 
    {
        e1++;
        // 读取E1A引脚电平判断方向
        if (!DL_GPIO_readPins(ENCODER_PORT, ENCODER_E1A_PIN)) 
        {
            Encoder_A.Should_Get_Encoder_Count++;  // 正转计数
        } 
        else 
        {
            Encoder_A.Should_Get_Encoder_Count--;  // 反转计数
        }
    }
    
    // 中断清除已处理的编码器中断标志位，计算完计数
    DL_GPIO_clearInterruptStatus(ENCODER_PORT, gpioA_interrupt);

    


    uint32_t portE2_intp = DL_GPIO_getEnabledInterruptStatus(MPU6050_PORT,MPU6050_INT_PIN_PIN);

	//MPU6050下降沿中断,频率可以通过DMP配置,默认200Hz
	if( (portE2_intp&MPU6050_INT_PIN_PIN)==MPU6050_INT_PIN_PIN )
	{
		Read_DMP();
		mpu6050.pitch = Roll;
		mpu6050.roll = Pitch;
		mpu6050.yaw = Yaw;
		mpu6050.gyro.x=gyro[0];
		mpu6050.gyro.y=gyro[1];
		mpu6050.gyro.z=gyro[2];
		mpu6050.accel.x=accel[0];
		mpu6050.accel.y=accel[0];
		mpu6050.accel.z=accel[0];
		//清空标志位
		DL_GPIO_clearInterruptStatus(MPU6050_PORT,MPU6050_INT_PIN_PIN);
	}

}





//电机控制函数
void MotorA_Set_Duty(unsigned int Duty)
{
    //ADUTY_SET(Duty);
    DL_TimerG_setCaptureCompareValue(PWM_0_INST, Duty, GPIO_PWM_0_C0_IDX);
}

void MotorB_Set_Duty(unsigned int Duty)
{
    //BDUTY_SET(Duty);
    DL_TimerG_setCaptureCompareValue(PWM_0_INST, Duty, GPIO_PWM_0_C1_IDX);
}

void Motor_dir(uint8_t motorA_dir, uint8_t motorB_dir) // 控制电机方向，1正，0反
{
    if (motorA_dir == 1){ADIR1;}
    else if (motorA_dir == 0){ADIR0;}
    if (motorB_dir == 1){BDIR1;}
    else if (motorB_dir == 0){BDIR0;}
}

